#include "core.hpp"
#include <future>
#include <atomic>

extern int running;

void CalcCore::exeI()
{
	switch (command.opcode)
	{
		// R Command,Command format: opcode,rd,rs1,rs2,func2
	case 0x01: // calc
		switch (command.imm)
		{
		case 0x01: // mov
			iregs[command.r1][priv] = iregs[command.r2][priv];
			break;
		case 0x02: // add
			iregs[command.r1][priv] = iregs[command.r2][priv] + iregs[command.r3][priv];
			break;
		case 0x04: // sub
			iregs[command.r1][priv] = iregs[command.r2][priv] - iregs[command.r3][priv];
			break;
		case 0x08: // neg
			iregs[command.r1][priv] = -int64_t(iregs[command.r2][priv]);
			break;
		default: // Illegal Instruction
			proper = { false, ERROR::IllegalInst };
			break;
		}
		break;
	case 0x02: // logic
		switch (command.imm)
		{
		case 0x01: // and
			iregs[command.r1][priv] = iregs[command.r2][priv] & iregs[command.r3][priv];
			break;
		case 0x02: // or
			iregs[command.r1][priv] = iregs[command.r2][priv] | iregs[command.r3][priv];
			break;
		case 0x04: // xor
			iregs[command.r1][priv] = iregs[command.r2][priv] ^ iregs[command.r3][priv];
			break;
		case 0x08: // not
			iregs[command.r1][priv] = ~iregs[command.r2][priv];
			break;
		case 0x10: // shl
			iregs[command.r1][priv] = iregs[command.r2][priv] << command.r3;
			break;
		case 0x20: // shr
			iregs[command.r1][priv] = iregs[command.r2][priv] >> command.r3;
			break;
		case 0x40: // sar
			iregs[command.r1][priv] = (int64_t)iregs[command.r2][priv] >> command.r3;
			break;
		default: // Illegal Instruction
			proper = { false, ERROR::IllegalInst };
			break;
		}
		break;
	case 0x03: // set
		switch (command.r2)
		{
		case 0x01: // mov
			iregs[command.r1][priv] = command.imm;
			break;
		case 0x02: // sign-extend
			if (command.imm & 0x200)
				iregs[command.r1][priv] = 0xffff'ffff'ffff'fc00 | command.imm;
			else
				iregs[command.r1][priv] = command.imm;
			break;
		case 0x04: // add with a reg
			iregs[command.r1][priv] = iregs[command.r2][priv] + command.imm;
			break;
		case 0x06://sub with a reg
			iregs[command.r1][priv] = iregs[command.r2][priv] - command.imm;
			break;
		case 0x08: // left shift
			iregs[command.r1][priv] = uint64_t(command.imm) << command.r3;
			break;
		case 0x0a://right shift
			iregs[command.r1][priv] = uint64_t(command.imm) >> command.r3;
			break;
		default: // Illegal Instruction
			proper = { false, ERROR::IllegalInst };
			break;
		}
		break;
	case 0x04: // load, only initiate MAR
		MAR = iregs[command.r2][priv] + command.imm;
		rwStat = true;
		break;
	case 0x05: // store, only initiate MAR and MDR
		MAR = iregs[command.r2][priv] + command.imm;
		MDR = iregs[command.r1][priv];
		rwStat = true;
		break;
	case 0x0a: // jmp
		if (command.r3 & EQ)
		{
			if (iregs[command.r1][priv] == 0)
				goto jump;
		}
		if (command.r3 & LT)
		{
			if (iregs[command.r1][priv] < 0)
				goto jump;
		}
		if (command.r3 & GT)
		{
			if (iregs[command.r1][priv] > 0)
				goto jump;
			else
				break;
		}
		else
			break;
	jump:
		if (command.r3 & ABS)
			sregs[Code][priv] = iregs[command.r2][priv] + command.imm;
		else
		{
			sregs[Code][priv] &= 0xffff'ffff'ffff'fc00;
			sregs[Code][priv] |= command.imm & 0x3ff;
		}
		break;
	case 0x0b: // call
		if (command.r3 & EQ)
		{
			if (iregs[command.r1][priv] == 0)
				goto call;
		}
		else if (command.r3 & LT)
		{
			if (iregs[command.r1][priv] != 0)
				goto call;
		}
		else if (command.r3 & GT)
		{
			if (iregs[command.r1][priv] > 0)
				goto call;
			else
				break;
		}
	call:
		// initiate MAR and MDR, to push RetAddr to stack
		MAR = iregs[Stack][priv] - 8;
		MDR = iregs[RetAddr][priv];
		rwStat = true;
		iregs[Stack][priv] -= 8;
		iregs[RetAddr][priv] = sregs[Code][priv];
		if (command.r3 & ABS)
			sregs[Code][priv] = iregs[command.r2][priv] + command.imm;
		else
		{
			sregs[Code][priv] &= 0xffff'ffff'ffff'fc00;
			sregs[Code][priv] |= command.imm & 0x3ff;
		}
		break;
	case 0x0c: // ret
		if (command.r3 & EQ)
		{
			if (iregs[command.r1][priv] == 0)
				goto ret;
		}
		else if (command.r3 & LT)
		{
			if (iregs[command.r1][priv] != 0)
				goto ret;
		}
		else if (command.r3 & GT)
		{
			if (iregs[command.r1][priv] > 0)
				goto ret;
			else
				break;
		}
	ret:
		// initiate MAR and MDR, to pop RetAddr from stack
		MAR = iregs[Stack][priv];
		rwStat = true;
		iregs[Stack][priv] += 8;
		sregs[Code][priv] = iregs[RetAddr][priv];
		break;
	case 0x0f: // trap
		if (priv)
			// Access Violation
			proper = { false, ERROR::AccessFault };
		sregs[Status][priv] |= Trap;
		sregs[Status][priv] |= uint64_t(command.imm) << 32;
		break;
	default: // Illegal Instruction
		proper = { false, ERROR::IllegalInst };
		break;
	}
	proper = { true, 0 };
}
void CalcCore::exeM()
{
	switch (command.opcode)
	{
	case 0x20: // mul
		iregs[command.r1][priv] = iregs[command.r2][priv] * iregs[command.r3][priv];
		break;
	case 0x21: // div
		iregs[command.r1][priv] = iregs[command.r2][priv] / iregs[command.r3][priv];
		break;
	case 0x22: // mod
		iregs[command.r1][priv] = iregs[command.r2][priv] % iregs[command.r3][priv];
		break;
	case 0x23: // mulh
		iregs[command.r1][priv] = (iregs[command.r2][priv] * iregs[command.r3][priv]) >> 32;
		break;
	case 0x24: // divh
		iregs[command.r1][priv] = (iregs[command.r2][priv] / iregs[command.r3][priv]) >> 32;
		break;
	default: // Illegal Instruction
		proper = {false, ERROR::IllegalInst};
		break;
	}
	proper = {true, 0};
}
void CalcCore::exeI_Sys()
{
	if (!priv) // Access Violation
		proper = {false, ERROR::AccessFault};
	switch (command.opcode)
	{
	case 0x70: // iret
		priv = false;
		break;
	case 0x71: // kload
		if (command.r2 < 4)
			iregs[command.r1][priv] = sregs[command.r2][priv];
		else
			iregs[command.r1][priv] = pages[command.r2 - 4];
		break;
	case 0x72: // kstore
		if (command.r2 < 4)
			sregs[command.r2][priv] = iregs[command.r1][priv];
		else
			pages[command.r2 - 4] = iregs[command.r1][priv];
		break;
	case 0x73: // read
		while (!bus.is_done())
			; // if bus not done, wait
		bus.set_direction(true);
		bus.set_data(0);
		bus.set_port(command.imm);
		bus.setup();
		rwStat = true;
		break;
	case 0x74: // write
		rwStat = true;
		break;
	case 0x7f: // halt
		sregs[Status][priv] ^= Running;
		break;
	default: // Illegal Instruction
		proper = {false, ERROR::IllegalInst};
		break;
	}
	proper = {true, 0};
}

void CalcCore::fetch()
{
	if (!(sregs[Status][priv] & Running))
		proper = {false, uint64_t(0xff00) << 32}; // Core Not Running
	if (sregs[Status][priv] & Block)
		proper = {false, uint64_t(0xff00) << 32}; // Core Not Running
	MAR = sregs[Code][priv] << 2;
	std::pair<bool, uint64_t> retval = addr_trans(&MAR, R | X | (priv ? P : 0), sregs[Status][priv], pages);
	memory(MAR, &MDR, 4, R | X);
	sregs[Code][priv]++;
	proper = {true, 0};
}
void CalcCore::decode()
{
	command.opcode = MDR & 0x7f;
	command.r1 = (MDR >> 7) & 0x1f;
	command.r2 = (MDR >> 12) & 0x1f;
	command.r3 = (MDR >> 17) & 0x1f;
	command.imm = (MDR >> 22) & 0x3ff;
}
void CalcCore::execute()
{
	if (command.opcode < 0x10)
		return exeI();
	else if (command.opcode >= 0x20 && command.opcode < 0x30)
		return exeM();
	else if (command.opcode >= 0x70 && command.opcode < 0x80)
		return exeI_Sys();
	else
		proper = {false, ERROR::IllegalInst};
}
void CalcCore::memrw()
{
	if (rwStat)
	{
		switch (command.opcode)
		{
		case 0x04: // load
			proper = addr_trans(&MAR, R | (priv ? P : 0), sregs[Status][priv], pages);
			if (proper.first)
			{
				memory(MAR, &MDR, command.r3, R);
				iregs[command.r1][priv] = MDR;
			}
			break;
		case 0x05: // store
			proper = addr_trans(&MAR, W | (priv ? P : 0), sregs[Status][priv], pages);
			if (proper.first)
				memory(MAR, &MDR, command.r3, W);
			break;
		case 0x0b: // call
			proper = addr_trans(&MAR, W | (priv ? P : 0), sregs[Status][priv], pages);
			if (proper.first)
				memory(MAR, &MDR, 8, W);
			break;
		case 0x0c: // ret
			proper = addr_trans(&MAR, R | (priv ? P : 0), sregs[Status][priv], pages);
			if (proper.first)
			{
				memory(MAR, &MDR, 8, R);
				iregs[RetAddr][priv] = MDR;
			}
			break;
		case 0x73: // read
			while (!bus.is_done())
				; // if bus not done, wait
			iregs[command.r1][priv] = bus.get_data();
			bus.clear();
			break;
		case 0x74: // write
			while (!bus.is_done())
				;
			bus.set_direction(false);
			bus.set_data(iregs[command.r1][priv],command.r2);
			bus.set_port(command.imm);
			bus.clear();
			break;
		}
	}
	rwStat = false;
}
void CalcCore::post()
{
	uint16_t intID = (sregs[Status][priv] >> 32) & 0x1ff; // get interrupt ID
	sregs[Status][priv] &= 0xffff'ffff;					  // clear interrupt ID
	if (sregs[Status][priv] & (Trap | Error | Masked))
	{
		if (sregs[Status][priv] & Error) // Error,always enable
			sregs[Status][priv] &= ~Error;
		else if (sregs[Status][priv] & Trap) // Trap,always enable
			sregs[Status][priv] &= ~Trap;
		else if (sregs[Status][priv] & Masked) // Outside ID,enable if IntEN
		{
			sregs[Status][priv] &= ~Masked;
			if (sregs[Status][priv] ^ IntEN) // Interrupt Disabled
				return;
		}
		MAR = sregs[IDTR][priv] + (uint64_t(intID) << 3);
		proper = addr_trans(&MAR, R | (priv ? P : 0), sregs[Status][priv], pages);
		if (proper.first) // goes correctly
		{
			memory(MAR, &MDR, 8, R);
			sregs[Code][priv] = MDR;
		}
		else								 // IDTR Wrong,system halt
			sregs[Status][priv] &= ~Running; // halt
	}
	else
	{
		priv = sregs[Status][priv] & Privilege;
		sregs[Status][1] |= Privilege;
		sregs[Status][0] &= ~(uint64_t)(Privilege);
	}
}

void CalcCore::init(Atomic<IOBus *> &bus)
{
	this->bus = bus;
	addr_trans.RvReg = {&sregs[RvBA][0], &sregs[RvBA][1]};
	reset();
}
void CalcCore::reset()
{
	for (int i = 0; i < 32; i++)
		iregs[i][0] = iregs[i][1] = 0;
	for (int i = 0; i < 4; i++)
		sregs[i][0] = sregs[i][1] = 0;
	for (int i = 0; i < 4; i++)
		pages[i] = 0;
	MAR = MDR = 0;
	iregs[Stack][0] = 0x10000;
	iregs[Stack][1] = 0x100000;
	sregs[Status][0] = Running | (ID == 0 ? Master : Slave);
	sregs[Status][1] = Running | Privilege | (ID == 0 ? Master : Slave);
	command = {0, 0, 0, 0, 0};
	priv = true;
	proper = {false, 0};
}
void CalcCore::operator()()
{
	fetch();
	if (!proper.first)
	{
		sregs[Status][priv] |= Error;
		sregs[Status][priv] |= proper.second;
		proper = {true, 0};
		goto error;
	}
	decode();
	execute();
	if (!proper.first)
	{
		sregs[Status][priv] |= Error;
		sregs[Status][priv] |= proper.second;
		proper = {true, 0};
		goto error;
	}
	memrw();
	if (!proper.first)
	{
		sregs[Status][priv] |= Error;
		sregs[Status][priv] |= proper.second;
		proper = {true, 0};
		goto error;
	}
error:
	post();
}